#pragma once

#include "cartographer/rcm/imu/preintegrate.h"
#include <ceres/ceres.h>

const int SIZE_POSE = 7;
const int SIZE_SPEEDBIAS = 9;

namespace cartographer
{
    class PoseUpdater : public ceres::LocalParameterization
    {
        virtual bool Plus(const double *x, const double *delta, double *x_plus_delta) const;

        virtual bool ComputeJacobian(const double *x, double *jacobian) const;

        virtual int GlobalSize() const
        {
            return 7;
        };

        virtual int LocalSize() const
        {
            return 6;
        };
    };

    class ImuOptNode
    {
    public:
        using Ptr = std::shared_ptr<ImuOptNode>;

        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        ImuOptNode()
            : Q(1.0, 0, 0, 0)
        {
            P.setZero();
            V.setZero();
            Ba.setZero();
            Bg.setZero();
        }

        inline void SetBias(const ImuBias &bias)
        {
            Ba = bias.acc_;
            Bg = bias.gyro_;
        }

        ImuOptNode(const double *pq, const double *vag)
        {
            P = Eigen::Vector3d(pq[0], pq[1], pq[2]);
            Q = Eigen::Quaterniond(pq[6], pq[3], pq[4], pq[5]);
            V = Eigen::Vector3d(vag[0], vag[1], vag[2]);
            Ba = Eigen::Vector3d(vag[3], vag[4], vag[5]);
            Bg = Eigen::Vector3d(vag[6], vag[7], vag[8]);
        }

        inline void SetArray(double *pq, double *vag)
        {
            pq[0] = P(0);
            pq[1] = P(1);
            pq[2] = P(2);
            pq[3] = Q.x();
            pq[4] = Q.y();
            pq[5] = Q.z();
            pq[6] = Q.w();

            vag[0] = V(0);
            vag[1] = V(1);
            vag[2] = V(2);
            vag[3] = Ba(0);
            vag[4] = Ba(1);
            vag[5] = Ba(2);
            vag[6] = Bg(0);
            vag[7] = Bg(1);
            vag[8] = Bg(2);
        }
        Eigen::Vector3d P;
        Eigen::Quaterniond Q;
        Eigen::Vector3d V;
        Eigen::Vector3d Ba;
        Eigen::Vector3d Bg;
        int64 time;
        ImuPreintegrate::Ptr integrator_ = nullptr;
    };

    class ImuOptEdge : public ceres::SizedCostFunction<15, 7, 9, 7, 9>
    {
    public:
        ImuOptEdge() = delete;

        ImuOptEdge(ImuPreintegrate *integrator)
            : integrator_(integrator)
        {
        }

        Eigen::Vector3d CalcPortionP(const ImuOptNode &poseI, const ImuOptNode &poseJ) const
        {
            double sum_dt = integrator_->sumTime_;
            return 0.5 * GRAVITY_POSITIVE * sum_dt * sum_dt + poseJ.P - poseI.P - poseI.V * sum_dt;
        }

        Eigen::Vector3d CalcPortionV(const ImuOptNode &poseI, const ImuOptNode &poseJ) const
        {
            double sum_dt = integrator_->sumTime_;
            return GRAVITY_POSITIVE * sum_dt + poseJ.V - poseI.V;
        }

        Eigen::Matrix<double, 15, 1> CalculateResidual(const ImuOptNode &poseI, const ImuOptNode &poseJ, const PVR &incre) const;

        virtual bool Evaluate(double const *const *params, double *residuals, double **jacobians) const
        {
            ImuOptNode poseI;
            poseI.P = Eigen::Vector3d(params[0][0], params[0][1], params[0][2]);
            poseI.Q = Eigen::Quaterniond(params[0][6], params[0][3], params[0][4], params[0][5]);
            poseI.V = Eigen::Vector3d(params[1][0], params[1][1], params[1][2]);
            poseI.Ba = Eigen::Vector3d(params[1][3], params[1][4], params[1][5]);
            poseI.Bg = Eigen::Vector3d(params[1][6], params[1][7], params[1][8]);

            ImuOptNode poseJ;
            poseJ.P = Eigen::Vector3d(params[2][0], params[2][1], params[2][2]);
            poseJ.Q = Eigen::Quaterniond(params[2][6], params[2][3], params[2][4], params[2][5]);
            poseJ.V = Eigen::Vector3d(params[3][0], params[3][1], params[3][2]);
            poseJ.Ba = Eigen::Vector3d(params[3][3], params[3][4], params[3][5]);
            poseJ.Bg = Eigen::Vector3d(params[3][6], params[3][7], params[3][8]);

            ImuBias biasNew(poseI.Ba, poseI.Bg);
            integrator_->SetNewBias(biasNew);
            PVR incre = integrator_->GetUpdatedDeltaPVR();
#if 0
        if ((poseI.Ba - integrator_->biasOrg_.acc_).norm() > 0.10 ||
            (poseI.Bg - integrator_->biasOrg_.gyro_).norm() > 0.01)
        {
            integrator_->repropagate(poseI.Ba, poseI.Bg);
        }
#endif
            Eigen::Map<Eigen::Matrix<double, 15, 1>> residual(residuals);
            residual = CalculateResidual(poseI, poseJ, incre);
            PrintResidual(residuals, 0);
            PrintResidual(residual, 0);
            Eigen::Matrix<double, 15, 15> infoMat;
            if (ImuOptions::GetInstance().imu_opt_use_identity_information)
            {
                infoMat.setIdentity();
            }
            else
            {
                infoMat = Eigen::LLT<Eigen::Matrix<double, 15, 15>>(integrator_->convariance_.inverse()).matrixL().transpose();
            }
            residual = infoMat * residual;
            PrintResidual(residuals, 1);
            PrintResidual(residual, 1);
            LOG(INFO) << "evval hasIntegrated " << integrator_->hasIntegrated;
            PrintMatrix("cov0", integrator_->convariance_);
            PrintMatrix("info0", infoMat);
            // error will accur
            if (jacobians)
            {
                LOG(INFO) << "evval[1] " << integrator_->GetPoseTime();
                double sum_dt = integrator_->sumTime_;
                Eigen::Matrix3d jPa = integrator_->jacobian_.block<3, 3>(OFF_P, OFF_BA);
                Eigen::Matrix3d jPg = integrator_->jacobian_.block<3, 3>(OFF_P, OFF_BG);
                Eigen::Matrix3d jQg = integrator_->jacobian_.block<3, 3>(OFF_R, OFF_BG);
                Eigen::Matrix3d jVa = integrator_->jacobian_.block<3, 3>(OFF_V, OFF_BA);
                Eigen::Matrix3d jVg = integrator_->jacobian_.block<3, 3>(OFF_V, OFF_BG);
                if (integrator_->jacobian_.maxCoeff() > 1e8 || integrator_->jacobian_.minCoeff() < -1e8)
                {
                    LOG(INFO) << "numerical unstable in preintegration\n";
                }
                LOG(INFO) << "evval[2] " << integrator_->GetPoseTime();
                // jacobians是残差公式的雅可比，参照下面公式
                // (1) residuals.block<3, 1>(OFF_P, 0) = poseI.Q.inverse() * CalcPortionP(poseI, poseJ) - incre.P;
                // (2) residuals.block<3, 1>(OFF_R, 0) = 2 * (delta_q.inverse() * (poseI.Q.inverse() * poseJ.Q)).vec();
                // (3) residuals.block<3, 1>(OFF_V, 0) = poseI.Q.inverse() * CalcPortionV(poseI, poseJ) - incre.V;
                // (4) residuals.block<3, 1>(OFF_BA, 0) = poseJ.Ba - poseI.Ba;
                // (5) residuals.block<3, 1>(OFF_BG, 0) = poseJ.Bg - poseI.Bg;
                if (jacobians[0])
                {
                    Eigen::Map<Eigen::Matrix<double, 15, 7, Eigen::RowMajor>> jacobian_pose_i(jacobians[0]);
                    jacobian_pose_i.setZero();
                    // 在公式(1)中对P求偏导,即为poseI.Q.inverse()
                    jacobian_pose_i.block<3, 3>(OFF_P, OFF_P) = -poseI.Q.inverse().toRotationMatrix();
                    // 在公式(1)中对Q求偏导
                    jacobian_pose_i.block<3, 3>(OFF_P, OFF_R) = cartographer::skewSymmetric(poseI.Q.inverse() * CalcPortionP(poseI, poseJ));
#if 0
            jacobian_pose_i.block<3, 3>(OFF_R, OFF_R) = -(poseJ.Q.inverse() * poseI.Q).toRotationMatrix();
#else
                    Eigen::Quaterniond original_delta_q(integrator_->dR);
                    Eigen::Quaterniond corrected_delta_q =
                        original_delta_q * cartographer::deltaQ(jQg * (poseI.Bg - integrator_->biasOrg_.gyro_));
                    jacobian_pose_i.block<3, 3>(OFF_R, OFF_R) =
                        -(cartographer::Qleft(poseJ.Q.inverse() * poseI.Q) * cartographer::Qright(corrected_delta_q))
                             .bottomRightCorner<3, 3>();
#endif
                    jacobian_pose_i.block<3, 3>(OFF_V, OFF_R) = cartographer::skewSymmetric(poseI.Q.inverse() * CalcPortionV(poseI, poseJ));
                    jacobian_pose_i = infoMat * jacobian_pose_i;
                    if (jacobian_pose_i.maxCoeff() > 1e8 || jacobian_pose_i.minCoeff() < -1e8)
                    {
                        LOG(INFO) << "numerical unstable in preintegration";
                        // std::cout << infoMat << std::endl;
                        // ROS_BREAK();
                    }
                }
                LOG(INFO) << "evval[3] " << integrator_->GetPoseTime();
                // PrintJacobians(jacobians, 1);
                if (jacobians[1])
                {
                    Eigen::Map<Eigen::Matrix<double, 15, 9, Eigen::RowMajor>> jacobian_speedbias_i(jacobians[1]);
                    jacobian_speedbias_i.setZero();
                    jacobian_speedbias_i.block<3, 3>(OFF_P, OFF_V - OFF_V) = -poseI.Q.inverse().toRotationMatrix() * sum_dt;
                    jacobian_speedbias_i.block<3, 3>(OFF_P, OFF_BA - OFF_V) = -jPa;
                    jacobian_speedbias_i.block<3, 3>(OFF_P, OFF_BG - OFF_V) = -jPg;
#if 0
            jacobian_speedbias_i.block<3, 3>(OFF_R, OFF_BG - OFF_V) = -jQg;
#else
                    // Eigen::Quaterniond corrected_delta_q = integrator_->dR * cartographer::deltaQ(jQg * (poseI.Bg -
                    // integrator_->biasOrg_.gyro_)); jacobian_speedbias_i.block<3, 3>(OFF_R, OFF_BG - OFF_V) =
                    // -cartographer::Qleft(poseJ.Q.inverse() * poseI.Q * corrected_delta_q).bottomRightCorner<3, 3>() * jQg;
                    Eigen::Quaterniond original_delta_q(integrator_->dR);
                    jacobian_speedbias_i.block<3, 3>(OFF_R, OFF_BG - OFF_V) =
                        -cartographer::Qleft(poseJ.Q.inverse() * poseI.Q * original_delta_q).bottomRightCorner<3, 3>() * jQg;
#endif
                    jacobian_speedbias_i.block<3, 3>(OFF_V, OFF_V - OFF_V) = -poseI.Q.inverse().toRotationMatrix();
                    jacobian_speedbias_i.block<3, 3>(OFF_V, OFF_BA - OFF_V) = -jVa;
                    jacobian_speedbias_i.block<3, 3>(OFF_V, OFF_BG - OFF_V) = -jVg;
                    jacobian_speedbias_i.block<3, 3>(OFF_BA, OFF_BA - OFF_V) = -Eigen::Matrix3d::Identity();
                    jacobian_speedbias_i.block<3, 3>(OFF_BG, OFF_BG - OFF_V) = -Eigen::Matrix3d::Identity();
                    jacobian_speedbias_i = infoMat * jacobian_speedbias_i;
                    // ROS_ASSERT(fabs(jacobian_speedbias_i.maxCoeff()) < 1e8);
                    // ROS_ASSERT(fabs(jacobian_speedbias_i.minCoeff()) < 1e8);
                }
                LOG(INFO) << "evval[4] " << integrator_->GetPoseTime();
                // PrintJacobians(jacobians, 2);
                if (jacobians[2])
                {
                    Eigen::Map<Eigen::Matrix<double, 15, 7, Eigen::RowMajor>> jacobian_pose_j(jacobians[2]);
                    jacobian_pose_j.setZero();
                    jacobian_pose_j.block<3, 3>(OFF_P, OFF_P) = poseI.Q.inverse().toRotationMatrix();
#if 0
            jacobian_pose_j.block<3, 3>(OFF_R, OFF_R) = Eigen::Matrix3d::Identity();
#else
                    Eigen::Quaterniond original_delta_q(integrator_->dR);
                    Eigen::Quaterniond corrected_delta_q =
                        original_delta_q * cartographer::deltaQ(jQg * (poseI.Bg - integrator_->biasOrg_.gyro_));
                    jacobian_pose_j.block<3, 3>(OFF_R, OFF_R) =
                        cartographer::Qleft(corrected_delta_q.inverse() * poseI.Q.inverse() * poseJ.Q).bottomRightCorner<3, 3>();
#endif
                    jacobian_pose_j = infoMat * jacobian_pose_j;
                    // ROS_ASSERT(fabs(jacobian_pose_j.maxCoeff()) < 1e8);
                    // ROS_ASSERT(fabs(jacobian_pose_j.minCoeff()) < 1e8);
                }
                LOG(INFO) << "evval[5] " << integrator_->GetPoseTime();
                // PrintJacobians(jacobians, 3);
                if (jacobians[3])
                {
                    Eigen::Map<Eigen::Matrix<double, 15, 9, Eigen::RowMajor>> jacobian_speedbias_j(jacobians[3]);
                    jacobian_speedbias_j.setZero();
                    jacobian_speedbias_j.block<3, 3>(OFF_V, OFF_V - OFF_V) = poseI.Q.inverse().toRotationMatrix();
                    jacobian_speedbias_j.block<3, 3>(OFF_BA, OFF_BA - OFF_V) = Eigen::Matrix3d::Identity();
                    jacobian_speedbias_j.block<3, 3>(OFF_BG, OFF_BG - OFF_V) = Eigen::Matrix3d::Identity();
                    jacobian_speedbias_j = infoMat * jacobian_speedbias_j;
                    // ROS_ASSERT(fabs(jacobian_speedbias_j.maxCoeff()) < 1e8);
                    // ROS_ASSERT(fabs(jacobian_speedbias_j.minCoeff()) < 1e8);
                }
                LOG(INFO) << "evval[6] " << integrator_->GetPoseTime();
                // PrintJacobians(jacobians, 4);
            }
            return true;
        }

    private:
        ImuPreintegrate *integrator_;
    };
} // namespace cartographer